/*
 * protocol.c
 *
 *  Created on: Jun 7, 2012
 *      Author: esl32
 */

#include "fcntl.h"
#include "protocol.h"

int parse_next(SerialConn* conn, unsigned char c)
{
   // implementation is by simple state machine 
   // documentation is given in the project report
   if (conn->state == START)
   {
      if (c == DLE)
         conn->state = FRAMEDLE;
   }
   else if (conn->state == FRAMEDLE)
   {
      if (c == DLE || c == ETX)
         conn->state = START;
      else
      {
         conn->state = DATA;
         conn->length = 1;
         conn->checksum = c;
         conn->packetBuffer[0] = c;
      }
   }
   else if (conn->state == DATA)
   {
      if (c == DLE)
      {
         conn->state = DATADLE;
      }
      else
      {
         conn->checksum ^= c;
         conn->packetBuffer[conn->length] = c;
         conn->length++;
      }
   }
   else if (conn->state == DATADLE)
   {
      if (c == DLE)
      {
         conn->checksum ^= c;
         conn->packetBuffer[conn->length] = c;
         conn->length++;
         conn->state = DATA;
      }
      else if (c == ETX)
      {
         conn->state = START;
         if (conn->checksum == 0)
         {
            return READY;
         }
         else
         {
            return CHECKSUM_INCORRECT;
         }
      }
      else
      {
         conn->state = START;
         return MALFORMED;
      }
   }
   return PARSING;
}

void write_packet(int fd, unsigned char* msg, int length)
{
   int i;
   char txBuffer[40];
   char check = 0;
   int cur_length = 1;
   txBuffer[0] = 0x10; // start byte
   for (i = 0; i < length; i++)
   {
      txBuffer[cur_length] = msg[i]; //add byte to the packet
      cur_length++;
      if (msg[i] == 0x10) //escape 0x10 by another 0x10
      {
         txBuffer[cur_length] = msg[i];
         cur_length++;
      }
      check ^= msg[i]; //calculate checksum
   }
   txBuffer[cur_length] = check; // add checksum
   cur_length++;
   if (check == 0x10) //if checksum is 0x10, then escape it
   {
	   txBuffer[cur_length] = check;
	   cur_length++;
   }
   txBuffer[cur_length] = 0x10; // end byte 1
   cur_length++;
   txBuffer[cur_length] = 0x03; //end byte 2
   cur_length++;

   write(fd, txBuffer, cur_length);
}

